#include <LitStdAfx.h>
#include "LitPhysicsPrereqs.h"
#include "LitPhysicsBody.h"

namespace Lit
{
	namespace Physics
	{
		Matrix::Matrix( const Lit::Vec3 & pos, const Lit::Quat& orient )
		{ mMatrix = Lit::Mat4( orient ); mMatrix.setTrans( pos ); }
		Matrix::Matrix( float* mat )
		{
			mMatrix = NewtonToOgre( mat ).mMatrix;
		}
		Matrix::Matrix(){ mMatrix = Lit::Mat4::IDENTITY; }

		float* Matrix::toNewton( ){
			Matrix::OgreToNewton( mMatrix, &mNMat[0][0] );
			return &mNMat[0][0];
		}
		
		Lit::Vec3 Matrix::getPosition() const{ return mMatrix.getTrans(); }
		Lit::Quat Matrix::getOrientation() const{ return mMatrix.extractQuaternion(); }

		void Matrix::setPosition( const Lit::Vec3 &pos ){ mMatrix.setTrans( pos ); }
		void Matrix::setOrientation( const Lit::Quat& orient ){ mMatrix = Matrix( mMatrix.getTrans(), orient ).mMatrix; }

		Matrix& Matrix::operator=( float *mat ){ mMatrix =  NewtonToOgre( mat ).mMatrix; return *this; }
		Matrix& Matrix::operator=( const Lit::Mat4& mat ){ mMatrix = mat; return *this; }
		Matrix& Matrix::operator=( const Matrix& mat ){ mMatrix = mat.mMatrix; return *this; }

		//! Take a Newton matrix and create a Quaternion + Position_vector
		void Matrix::MatrixToQuatPos( const float* matrix, Ogre::Quaternion& quat, Ogre::Vector3 &pos )
		{
			// this takes a matrix returned by Newton, and creates a Quaternion
			// and position Vector3, which is more meaningful for Ogre.
			using namespace Ogre;
			quat = Quaternion( Matrix3(	matrix[0], matrix[4], matrix[8],
							matrix[1], matrix[5], matrix[9],
							matrix[2], matrix[6], matrix[10] ) );

			pos = Vector3( matrix[12], matrix[13], matrix[14] );
		}

		//! Take a Quaternion and Position Matrix and create a Newton-happy float matrix!
		void Matrix::QuatPosToMatrix( const Ogre::Quaternion& quat, const Ogre::Vector3 &pos, float* matrix )
		{
			// this takes a Quaternion and a Vector3 and creates a float array
			// which is more meaningful to Newton.
			using namespace Ogre;
			Matrix3 rot;
			Vector3 xcol, ycol, zcol;

			quat.ToRotationMatrix( rot );	// creates a 3x3 rotation matrix from the Quaternion.

			xcol = rot.GetColumn(0);
			ycol = rot.GetColumn(1);
			zcol = rot.GetColumn(2);

			// now fill the final matrix with the appropriate data:
			matrix[0] = xcol.x;
			matrix[1] = xcol.y;
			matrix[2] = xcol.z;
			matrix[3] = 0.0f;

			matrix[4] = ycol.x;
			matrix[5] = ycol.y;
			matrix[6] = ycol.z;
			matrix[7] = 0.0f;

			matrix[8] = zcol.x;
			matrix[9] = zcol.y;
			matrix[10] = zcol.z;
			matrix[11] = 0.0f;

			matrix[12] = pos.x;
			matrix[13] = pos.y;
			matrix[14] = pos.z;
			matrix[15] = 1.0;
		}

		Matrix Matrix::NewtonToOgre( const float* matrix_in )
		{
			// from Newton to Ogre::Matrix4
			Matrix ret;
			ret.mMatrix = Ogre::Matrix4( matrix_in[0], matrix_in[4], matrix_in[8], matrix_in[12],
				matrix_in[1], matrix_in[5], matrix_in[9], matrix_in[13],
				matrix_in[2], matrix_in[6], matrix_in[10], matrix_in[14],
				matrix_in[3], matrix_in[7], matrix_in[11], matrix_in[15] );
			return ret;
		}
		// Matrix4x4
		void Matrix::OgreToNewton( const Ogre::Matrix4& matrix_in, float* matrix_out )
		{
			// from Ogre to Newton.
			matrix_out[0] = matrix_in[0][0];
			matrix_out[1] = matrix_in[1][0];
			matrix_out[2] = matrix_in[2][0];
			matrix_out[3] = matrix_in[3][0];

			matrix_out[4] = matrix_in[0][1];
			matrix_out[5] = matrix_in[1][1];
			matrix_out[6] = matrix_in[2][1];
			matrix_out[7] = matrix_in[3][1];

			matrix_out[8] = matrix_in[0][2];
			matrix_out[9] = matrix_in[1][2];
			matrix_out[10] = matrix_in[2][2];
			matrix_out[11] = matrix_in[3][2];

			matrix_out[12] = matrix_in[0][3];
			matrix_out[13] = matrix_in[1][3];
			matrix_out[14] = matrix_in[2][3];
			matrix_out[15] = matrix_in[3][3];
		}
		LIT_API std::ostream& operator << ( std::ostream & strm, const Force& f ){
			strm << Lit::String("direction: ")
				(f.getDirection())
				(", force(")(f.hasForce())("): ")(f.getForce())
				(", velocity(")(f.hasVelocity())("): ")(f.getVelocity())
				(", torque(")(f.hasTorque())("): ")(f.getTorque())
				(")").str();
			return strm;
		}
		LIT_API const Force Force::ZERO;
		 
		Force::Force( const Lit::Vec3& force )
		{ reset(); setComposed(force); }
		
		Force::Force( const Lit::Vec3& direction, Lit::Real force, Lit::Real velocity, Lit::Real torque, size_t flags )
		{ set( direction, force, velocity, torque, flags ); }
		
		Force::Force( const Force &rk )
		{ mDirection = rk.mDirection; mForce = rk.mForce; mTorque = rk.mTorque; mVelocity = rk.mVelocity; }
		
		Force::Force( )
		{ reset(); }
		Force::~Force()
		{ }
			

		Force& Force::operator=( const Force &rk ){ set( rk.mDirection, rk.mForce, rk.mVelocity, rk.mTorque, rk.mFlags ); return *this; }

		bool Force::operator==( const Force& rk ) const{ return (mDirection == rk.mDirection && mForce == rk.mForce && mTorque == rk.mTorque && mVelocity == rk.mVelocity); }
		bool Force::operator!=( const Force& rk ) const{ return (mDirection != rk.mDirection || mForce != rk.mForce || mTorque != rk.mTorque || mVelocity != rk.mVelocity); }
			
		
		bool Force::hasForce() const{ return hasFlag( USES_FORCE ); }
		bool Force::hasVelocity() const{ return hasFlag( USES_VELOCITY ); }
		bool Force::hasTorque() const{ return hasFlag( USES_TORQUE ); }
		bool Force::hasNothing() const{ return mFlags == USES_NOTHING; }

		size_t Force::getFlags() const{ return mFlags; }
		bool Force::hasFlag( ForceFlags f ) const{ return ((mFlags&f) != 0); }
		void Force::setFlags( size_t flags ){ mFlags = flags; }
		
			
		void Force::reset(){ mForce = mTorque = mVelocity = 0.0f; mDirection = Lit::Vec3::ZERO; mFlags = USES_NOTHING; }
		void Force::set( const Lit::Vec3& direction, Lit::Real force, Lit::Real velocity, Lit::Real torque, size_t flags ){ 
			reset(); 
			setComposed( direction*force );
			mVelocity = velocity;
			mTorque = torque;
			mFlags = flags;
		}
		void Force::setComposed( const Lit::Vec3& direction_mul_force )
		{ 
			mDirection = direction_mul_force;
			mForce = mDirection.normalise();
			mFlags = USES_FORCE;
		}
		
		void Force::setForce( Lit::Real fo ){ mForce = fo; mFlags |= USES_FORCE; }
		void Force::setVelocity( Lit::Real v ){ mVelocity = v; mFlags |= USES_VELOCITY; }
		void Force::setTorque( Lit::Real t ){ mTorque = t; mFlags |= USES_TORQUE; }
		void Force::setDirection( const Lit::Vec3& dir ){ mDirection = dir; mDirection.normalise(); }

		const Lit::Vec3 &Force::getDirection() const{ return mDirection; }
		Lit::Real Force::getVelocity() const{ return mVelocity; }
		Lit::Real Force::getTorque() const{ return mTorque; }
		Lit::Real Force::getForce() const{ return mForce; }
			
		Lit::Vec3 Force::getRealVelocity() const{ return mDirection*mVelocity; }
		Lit::Vec3 Force::getRealTorque() const{ return mDirection*mTorque; }
		Lit::Vec3 Force::getRealForce() const{ return mDirection*mForce; }
		
		 Lit::Vec3 Force::getAppliedVelocity( float mass ) const{ return mass * mDirection * mVelocity; }
		Lit::Vec3 Force::getAppliedTorque( float mass ) const{ return mass * mDirection * mTorque; }
		Lit::Vec3 Force::getAppliedForce( float mass ) const{ return mass * mDirection * mForce; }

		ForceList::ForceList(){}
		ForceList::ForceList( const ForceList& rk ){ _d = rk._d; }
		ForceList::~ForceList(){}

		void ForceList::clear(){_d.clear(); }
		ForceList& ForceList::push( const Force& f ){_d.push_back(f); return *this; }
		ForceList& ForceList::pop(){ if(!_d.empty()) _d.pop_back();  return *this; }

		ForceList::type& ForceList::data(){ return _d; }
		const ForceList::type& ForceList::data() const{ return _d; }
			
		ForceList::iterator ForceList::begin(){ return iterator(_d, _d.begin()); }
		ForceList::const_iterator ForceList::begin() const{ return const_iterator(_d, _d.begin()); }

		ForceList::iterator ForceList::end(){ return iterator(_d, _d.end()); }
		ForceList::const_iterator ForceList::end() const{ return const_iterator(_d, _d.end()); }

		bool ForceList::empty(){ return _d.empty(); }
		size_t ForceList::count() const{ return _d.size(); }
		size_t ForceList::size() const{ return _d.size(); }
		
		ForceList::Result ForceList::calculateExternalForces( const Body * body )
		{
			ForceList::Result res;
			for( auto i=_d.begin(); i!=_d.end(); i++ )
			{
				if( i->hasForce() ){ res.has_force = true; res.force += Lit::Vec4(i->getForce()*i->getDirection()*body->getMass()); }
				if( i->hasTorque() ){ res.has_torque = true; res.torque += Lit::Vec4(i->getTorque()*i->getDirection()); }
				if( i->hasVelocity() ){ res.has_veloc = true; res.veloc += Lit::Vec4(i->getVelocity()*i->getDirection()); }
			}
			return res;
		}
	};
};